﻿#region COPYRIGHT
//--------------------------------------------------------------------------------
// <copyright file="Vector3Extensions.cs" company="starLiGHT Entertainment Studios">
//    Copyright (c) 2007, 2008, 2009, 2010 
//       Roland Rosenkranz (Glatzemann@email.de)
// </copyright>
// <license>
//   This file is part of starLiGHT.Core.
//
//   starLiGHT.Core is free software: you can redistribute it and/or modify
//   it under the terms of the GNU Lesser General Public License as published by
//   the Free Software Foundation, either version 3 of the License, or
//   (at your option) any later version.
//
//   starLiGHT.Core is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//   GNU Lesser General Public License for more details.
//
//   You should have received a copy of the GNU Lesser General Public License
//   along with starLiGHT.Core. If not, see http://www.gnu.org/licenses/.
//    
//   ADDITIONAL (commercial) LICENSES for starLiGHT.Core are available on request.
// </license>
// <version>
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 620                   $:  Revision of last commit *
// * $Author:: unknown            $:  Author of last commit   *
// * $Date:: 2010-12-07 15:23:58 #$:  Date of last commit     *
// ************************************************************
// </version>
//--------------------------------------------------------------------------------
#endregion

namespace starLiGHT
{
    #region Using Statements
    using System;
    using System.Collections.Generic;
    using System.Text;
    using Microsoft.Xna.Framework;

    #endregion

    /// <summary>
    /// Extension methods for the Vector3 data type
    /// </summary>
    public static class Vector3Extensions
    {
        public static bool IsValid(this Vector3 v)
        {
            return !float.IsNaN(v.X) && !float.IsInfinity(v.X) &&
                   !float.IsNaN(v.Y) && !float.IsInfinity(v.Y) &&
                   !float.IsNaN(v.Z) && !float.IsInfinity(v.Z);
        }

        public static bool IsZeroLength(this Vector3 v)
        {
            return (v.X * v.X) + (v.Y * v.Y) + (v.Z * v.Z) < (1e-06 * 1e-06);
        }

        public static Quaternion getRotationTo(this Vector3 v, Vector3 dest)
        {
            return v.getRotationTo(dest, Vector3.Zero);
        }

        public static Quaternion getRotationTo(this Vector3 v, Vector3 dest, Vector3 fallbackAxis)
        {
            // Based on Stan Melax's article in Game Programming Gems
            Quaternion q = new Quaternion();
            
            // Copy, since cannot modify local
            Vector3 v0 = v;
            Vector3 v1 = dest;
            v0.Normalize();
            v1.Normalize();

            float d = Vector3.Dot(v0, v1);
            
            // If dot == 1, vectors are the same
            if (d >= 1.0f)
            {
                return Quaternion.Identity;
            }

            if (d < (1e-6f - 1.0f))
            {
                if (fallbackAxis != Vector3.Zero)
                {
                    // rotate 180 degrees about the fallback axis
                    q = Quaternion.CreateFromAxisAngle(fallbackAxis, (float)Math.PI);
                }
                else
                {
                    // Generate an axis
                    Vector3 axis = Vector3.Cross(Vector3.UnitX, v);

                    // pick another if colinear
                    if (axis.IsZeroLength()) 
                    {
                        axis = Vector3.Cross(Vector3.UnitY, v);
                    }

                    axis.Normalize();
                    q = Quaternion.CreateFromAxisAngle(axis, (float)Math.PI);
                }
            }
            else
            {
                float s = (float)Math.Sqrt((1 + d) * 2);
                float invs = 1 / s;

                Vector3 c = Vector3.Cross(v0, v1);

                q.X = c.X * invs;
                q.Y = c.Y * invs;
                q.Z = c.Z * invs;
                q.W = s * 0.5f;
                q.Normalize();
            }

            return q;
        }

        public static Vector3 Multiply(this Vector3 v, float a)
        {            
            return new Vector3(v.X * a, v.Y * a, v.Z * a);        
        }

        public static Vector3 Floor(this Vector3 v, Vector3 cmp)
        {
            if (cmp.X < v.X)
            {
                v.X = cmp.X;
            }

            if (cmp.Y < v.Y)
            {
                v.Y = cmp.Y;
            }

            if (cmp.Z < v.Z)
            {
                v.Z = cmp.Z;
            }

            return v;
        }

        public static Vector3 Ceil(this Vector3 v, Vector3 cmp)
        {
            if (cmp.X > v.X)
            {
                v.X = cmp.X;
            }

            if (cmp.Y > v.Y)
            {
                v.Y = cmp.Y;
            }

            if (cmp.Z > v.Z)
            {
                v.Z = cmp.Z;
            }

            return v;
        }

        public static float Item(this Vector3 v, int index)
        {
            if (index == 0)
            {
                return v.X;
            }
            else if (index == 1)
            {
                return v.Y;
            }
            else if (index == 2)
            {
                return v.Z;
            }
            else
            {
                throw new ArgumentException("invalid index (0..2) for a vector3");
            }
        }

        public static void Item(this Vector3 v, int index, float value)
        {
            if (index == 0)
            {
                v.X = value;
            }
            else if (index == 1)
            {
                v.Y = value;
            }
            else if (index == 2)
            {
                v.Z = value;
            }
            else
            {
                throw new ArgumentException("invalid index (0..2) for a vector3");
            }
        }
    }
}
